﻿using System;
using System.Collections.Generic;
using System.Text;
using G3.State;
using dnAnalytics.LinearAlgebra;
using System.IO;

namespace G3.Controller
{
    /// <summary>
    /// Describes a field on the map
    /// </summary>
    /// 
    public struct Field
    {        
        public System.Drawing.Color color;   //szín
        public double grip;                  // tapadás, [0,1]
        public double friction;              // súrlódás, ami lassít [0,1]
        //public double maxSpeed;            // max sebesség
        public double penalty;               // büntetés, [0,végtelen)
        public System.Drawing.Bitmap texture;

        

        public Field(System.Drawing.Color color, double grip, double friction, double penalty, String path)
        {
            this.color = color;
            this.grip = grip;
            this.friction = friction;
            this.penalty = penalty;
            string filePath = System.Windows.Forms.Application.StartupPath + "\\Data\\" + path;            
            this.texture = new System.Drawing.Bitmap(filePath);
            
        }
    }
     
    public static class Map
    {
        private static Field[] surfaces;        
        /// <summary>
        /// Gets the surface types, which are defined in the parameter file.
        /// </summary>
        public static Field[] Surfaces
        {
            get { return surfaces; }
        }

        private static int[,] fields;
        /// <summary>
        /// Gets the whole map.
        /// </summary>
        public static int[,] Fields
        {
            get { return fields; }
        }

        private static int mapWidth;
        public static int MapWidth
        {
            get { return mapWidth; }
        }

        private static int mapHeight;
        public static int MapHeight
        {
            get { return mapHeight; }
        }

        private static Pair startPos;
        public static Pair StartPos
        {
            get { return startPos; }
        }

        private static Pair finishPos;
        public static Pair FinishPos
        {
            get { return finishPos; }
        }

        private static double finishRadius;
        public static double FinishRadius
        {
            get { return finishRadius; }
        }

        private static String imagePath;
        public static String ImagePath
        {
            get { return imagePath; }
        }

        public static Field GetSurface(int x, int y)
        {
            return surfaces[fields[x, y]-1];
        }

        private static bool loaded = false;

        public static void InitSurfaces()
        {
            surfaces = new Field[7];
            //TODO:: if the penalty is infinity....

            //Tarmac
            surfaces[0] = new Field(System.Drawing.Color.FromArgb(int.Parse(Parameters.GetParameter("TarmacColorR")), int.Parse(Parameters.GetParameter("TarmacColorG")), int.Parse(Parameters.GetParameter("TarmacColorB"))),
                                    double.Parse(Parameters.GetParameter("TarmacGrip")),
                                    double.Parse(Parameters.GetParameter("TarmacFriction")),
                                    double.Parse(Parameters.GetParameter("TarmacPenalty")),
                                    Parameters.GetParameter("TarmacTexture"));

            //Gravel
            surfaces[1] = new Field(System.Drawing.Color.FromArgb(int.Parse(Parameters.GetParameter("GravelColorR")), int.Parse(Parameters.GetParameter("GravelColorG")), int.Parse(Parameters.GetParameter("GravelColorB"))),
                                    double.Parse(Parameters.GetParameter("GravelGrip")),
                                    double.Parse(Parameters.GetParameter("GravelFriction")),
                                    double.Parse(Parameters.GetParameter("GravelPenalty")),
                                    Parameters.GetParameter("GravelTexture"));
            
            //Mud
            surfaces[2] = new Field(System.Drawing.Color.FromArgb(int.Parse(Parameters.GetParameter("MudColorR")), int.Parse(Parameters.GetParameter("MudColorG")), int.Parse(Parameters.GetParameter("MudColorB"))),
                                    double.Parse(Parameters.GetParameter("MudGrip")),
                                    double.Parse(Parameters.GetParameter("MudFriction")),
                                    double.Parse(Parameters.GetParameter("MudPenalty")),
                                    Parameters.GetParameter("MudTexture"));
            
            //Grass
            surfaces[3] = new Field(System.Drawing.Color.FromArgb(int.Parse(Parameters.GetParameter("GrassColorR")), int.Parse(Parameters.GetParameter("GrassColorG")), int.Parse(Parameters.GetParameter("GrassColorB"))),
                                    double.Parse(Parameters.GetParameter("GrassGrip")),
                                    double.Parse(Parameters.GetParameter("GrassFriction")),
                                    double.Parse(Parameters.GetParameter("GrassPenalty")),
                                    Parameters.GetParameter("GrassTexture"));
            
            //Wood
            surfaces[4] = new Field(System.Drawing.Color.FromArgb(int.Parse(Parameters.GetParameter("WoodColorR")), int.Parse(Parameters.GetParameter("WoodColorG")), int.Parse(Parameters.GetParameter("WoodColorB"))),
                                    double.Parse(Parameters.GetParameter("WoodGrip")),
                                    double.Parse(Parameters.GetParameter("WoodFriction")),
                                    double.Parse(Parameters.GetParameter("WoodPenalty")),
                                    Parameters.GetParameter("WoodTexture"));
            
            //Start
            surfaces[5] = new Field(System.Drawing.Color.FromArgb(int.Parse(Parameters.GetParameter("StartColorR")), int.Parse(Parameters.GetParameter("StartColorG")), int.Parse(Parameters.GetParameter("StartColorB"))),
                                    1.0,
                                    1.0,
                                    0.0,
                                    Parameters.GetParameter("StartTexture"));
            
            //Finish
            surfaces[6] = new Field(System.Drawing.Color.FromArgb(int.Parse(Parameters.GetParameter("FinishColorR")), int.Parse(Parameters.GetParameter("FinishColorG")), int.Parse(Parameters.GetParameter("FinishColorB"))),
                                    1.0,
                                    1.0,
                                    0.0,
                                    Parameters.GetParameter("FinishTexture"));
        }

        public static void LoadMap(String path)
        {
            imagePath = path;
            System.Drawing.Bitmap source = new System.Drawing.Bitmap(imagePath);
            mapHeight = source.Height;
            mapWidth = source.Width;
            InitSurfaces();
            
            //Generating map            
            fields = new int[mapWidth,mapHeight];
            for (int x = 0; x != mapWidth; ++x)
            {
                for (int y = 0; y != mapHeight; ++y)
                {
                    System.Drawing.Color currentPixel = source.GetPixel(x, y);
                    int i=0;
                    while(i<7 && currentPixel!=surfaces[i].color)
                    {
                        ++i;
                    }
                    if (currentPixel == surfaces[i].color)
                    {
                        fields[x, y] = i + 1;
                        if (currentPixel == surfaces[5].color) 
                            startPos = new Pair(x, y);
                        else if (currentPixel == surfaces[6].color) 
                            finishPos = new Pair(x, y);
                        else 
                            ;
                    }
                    else throw new Exception("Bad color in source bitmap file: " + currentPixel.ToString());
                }
            }

            //TODO:Start,FinisPos
            loaded = true;
        }

        public static Pair StateCoordsToMapCoords(Pair input)
        {
            return new Pair(input.x, -input.y);
        }

        public static Pair MapCoordsToStateCoords(Pair input)
        {
            return new Pair(input.x, -input.y );
        }

        

        /// <summary>
        /// The estimated best position at the current time calcuted from the previous position(s)
        /// </summary>
        /// <returns></returns>
        public static Pair GetOptimalPosition()
        {
            int depth = 0;  // ~ when have made the wrong decision
            Pair currentPos = StateCoordsToMapCoords(Controller.GetOnlyInstance().AttachedState.CurrentStateVars.carPosition);
            
            while(true)
            {
                Pair basePos = StateCoordsToMapCoords(Controller.GetOnlyInstance().AttachedState.GetStateVars(depth - 1).carPosition);
                if ((basePos.x == 0 && basePos.y == 0) || depth==-1) return currentPos;
                Pair distance = currentPos - basePos;
                double orientation = Controller.GetOnlyInstance().AttachedState.GetStateVars(depth - 1).carOrientation - 180;

                Pair curr = new Pair(basePos.x + Math.Sin(orientation * Math.PI / 180) * distance.x,
                                     basePos.y + Math.Cos(orientation * Math.PI / 180) * distance.y);
                if (fields[(int)Math.Truncate(curr.x), (int)Math.Truncate(curr.y)] == 1)
                {
                    return (new Pair(basePos.x + Math.Sin(orientation * Math.PI / 180) * (distance.x * 1.8),
                                     basePos.y + Math.Cos(orientation * Math.PI / 180) * (distance.y * 1.8)));
                }

                for (int angle = 1; angle < 90; angle+=2)
                {
                    curr = new Pair(basePos.x + Math.Sin((orientation + angle) * Math.PI / 180) * distance.x,
                                    basePos.y + Math.Cos((orientation + angle) * Math.PI / 180) * distance.y);
                    if (fields[(int)Math.Truncate(curr.x), (int)Math.Truncate(curr.y)]==1) return curr;

                    curr = new Pair(basePos.x + Math.Sin((orientation - angle) * Math.PI / 180) * distance.x,
                                    basePos.y + Math.Cos((orientation - angle) * Math.PI / 180) * distance.y);
                    if (fields[(int)Math.Truncate(curr.x), (int)Math.Truncate(curr.y)]==1) return curr;

                    distance.x *= 1;
                }

                --depth;                
            }
        }

        public static int[,] GetFieldOfVision(Pair carPosition, double carOrientationRad)
        {
            //if (!loaded) throw new Exception("map has not been loaded yet");
            int width = int.Parse((String)Parameters.GetParameter("FieldOfVisionWidth"));
            int height = int.Parse((String)Parameters.GetParameter("FieldOfVisionHeight"));
            int[,] fov = new int[width, height];
            carOrientationRad -= Math.PI;
            Pair LeftBottom = new Pair(carPosition.x - Math.Cos(carOrientationRad) * (width / 2),
                                         carPosition.y - Math.Sin(carOrientationRad) * (width / 2));
            for (int x = 0; x != width; ++x)
            {
                for (int y = 0; y != height; ++y)
                {
                    Pair RealPosition = new Pair();
                    Quad Rotation = new Quad(Math.Cos(carOrientationRad), -Math.Sin(carOrientationRad), Math.Sin(carOrientationRad), Math.Cos(carOrientationRad));
                    RealPosition = LeftBottom + Rotation * (new Pair(x, y));
                    int RX = (int)Math.Truncate(RealPosition.x);
                    int RY = (int)Math.Truncate(RealPosition.y);
                    if (RX < 0 || RY < 0 || RX > fields.GetUpperBound(0) || RY > fields.GetUpperBound(1)) fov[width - 1 - x, height - 1 - y] = 5; //térképen kívüli rész járhatatlan
                    else fov[width - 1 - x, height - 1 - y] = fields[RX, RY];
                }
            }
            return fov;
        }

        public static Vector GetControlPoints(Pair carPosition, double carOrientationRad, int num)
        {
            //Return value: relative positions of the controlpoints from the previous point

            int[,] fov = GetFieldOfVision(carPosition, carOrientationRad);
            //int num = int.Parse((String)Parameters.GetParameter("NumberOfControlPoints"));
            double space = int.Parse((String)Parameters.GetParameter("FieldOfVisionHeight")) / (num + 1);
            Pair[] cp = new Pair[num];
            Pair[] temp = new Pair[num];
            double RunningOutRoadCorrection = double.Parse((String)Parameters.GetParameter("RunningOutRoadCorrection"));
            bool broken = false;

            for (int i = 0; i != num; ++i)
            {
                int y = fov.GetUpperBound(1) - (int)space * (i + 1);
                int x = 0;
                int left, right;

                //-- searching for road
                while (x <= fov.GetUpperBound(0) && fov[x, y] != 1) ++x;            //searching for left side of the road               
                if (x == fov.GetUpperBound(0) + 1 || broken)                                  //there isn't any tarmac
                {
                    cp[i] = new Pair(0, space);
                    broken = true;
                }
                else
                {
                    left = x;
                    while (x <= fov.GetUpperBound(0) && fov[x, y] == 1) ++x;        //searching for right side of the road
                    if (x == fov.GetUpperBound(0) + 1) right = x - 1;                    //right side isn't in the field of vision
                    else right = x;

                    if (left == 0 && right != fov.GetUpperBound(0))                 //a side isn't visible -> in that direction the road may continue
                    {
                        right -= (int)Math.Truncate(RunningOutRoadCorrection * right);
                    }
                    else if (left != 0 && right == fov.GetUpperBound(0))
                    {
                        left += (int)Math.Truncate(RunningOutRoadCorrection * (fov.GetUpperBound(0) - left));
                    }

                    //determining controlpoint
                    temp[i] = new Pair((left + right) / 2, space);

                    if (i == 0) cp[0] = new Pair((left + right) / 2 - fov.GetUpperBound(0) / 2, space);
                    else cp[i] = new Pair((left + right) / 2 - temp[i - 1].x, space);
                }

            }

            //return cp;

            Vector result = VectorBuilder.CreateVector(cp.GetLength(0));
            for (int i = 0; i != cp.GetLength(0); ++i)
            {
                result[i] = cp[i].x;
            }
            return result;
        }


        //public static Matrix GetFieldOfVision(Pair carPosition, double carOrientationRad)
        //{
        //    if (!loaded) throw new Exception("map has not been loaded yet");
        //    int width = int.Parse((String)Parameters.GetParameter("FieldOfVisionWidth"));
        //    int height = int.Parse((String)Parameters.GetParameter("FieldOfVisionHeight"));
        //    Matrix fov = MatrixBuilder.CreateMatrix(width, height);
        //    carOrientationRad -= Math.PI;
        //    Pair LeftBottom = new Pair(carPosition.x - Math.Cos(carOrientationRad) * (width / 2),
        //                                 carPosition.y - Math.Sin(carOrientationRad) * (height / 2)); //BUG
        //    Quad Rotation = new Quad(Math.Cos(carOrientationRad), -Math.Sin(carOrientationRad), Math.Sin(carOrientationRad), Math.Cos(carOrientationRad));
        //    for (int x = 0; x != width; ++x)
        //    {
        //        for (int y = 0; y != height; ++y)
        //        {
        //            Pair RealPosition = new Pair();
        //            RealPosition = LeftBottom + Rotation * (new Pair(x, y));
        //            int RX = (int)Math.Truncate(RealPosition.x);
        //            int RY = (int)Math.Truncate(RealPosition.y);
        //            if (RX < 0 || RY < 0 || RX > fields.GetUpperBound(0) || RY > fields.GetUpperBound(1)) fov[width - 1 - x, height - 1 - y] = 5; //térképen kívüli rész járhatatlan
        //            else fov[width - 1 - x, height - 1 - y] = fields[RX, RY];
        //        }
        //    }
        //    return fov;
        //}

        /// <summary>
        /// Gets the controlpoints in the current field of vision.
        /// </summary>
        /// <param name="carPosition"></param>
        /// <param name="carOrientationRad"></param>
        /// <returns>Vector of the relative x positions</returns>
        //public static Vector GetControlPoints(Pair carPosition, double carOrientationRad)
        //{   
        //    Matrix fov = GetFieldOfVision(carPosition, carOrientationRad);
        //    int num = int.Parse((String)Parameters.GetParameter("NumberOfControlPoints"));
        //    double space = int.Parse((String)Parameters.GetParameter("FieldOfVisionHeight")) / (num+1);
        //    //Pair[] cp = new Pair[num];
        //    //Pair[] temp = new Pair[num];
        //    Vector cp = VectorBuilder.CreateVector(num);
        //    Vector temp = VectorBuilder.CreateVector(num);
        //    double RunningOutRoadCorrection = double.Parse((String)Parameters.GetParameter("RunningOutRoadCorrection"));
        //    bool broken = false;

        //    for(int i=0; i != num; ++i)
        //    {
        //        int y = fov.Columns - (int)space * (i+1);
        //        int x = 0;
        //        int left, right;

        //        //-- searching for road
        //        while (x < fov.Rows && fov[x, y] != 1) ++x;             //searching for left side of the road               
        //        if (x == fov.Rows + 1)                                  //there isn't any tarmac ||broken
        //        {
        //            //cp[i] = new Pair(0,space);
        //            cp[i] = 0;
        //            broken = true;
        //        }
        //        else
        //        {
        //            left = x;
        //            while (x < fov.Rows && fov[x, y] == 1) ++x;        //searching for right side of the road
        //            if(x == fov.Rows+1) right = x-1;                    //right side isn't in the field of vision
        //            else right=x;

        //            if (left == 0 && right != fov.Rows)                 //a side isn't visible -> in that direction the road may continue
        //            {
        //                right -= (int)Math.Truncate(RunningOutRoadCorrection * right);
        //            }
        //            else if (left != 0 && right == fov.Rows)
        //            {
        //                left += (int)Math.Truncate(RunningOutRoadCorrection * (fov.Rows - left));
        //            }

        //            //determining controlpoint
        //            //temp[i] = new Pair((left + right) / 2, space);
        //            temp[i] = (left + right) / 2;                    
        //            //if (i==0) cp[0] = new Pair((left+right)/2 - fov.GetUpperBound(0)/2,space);
        //            //else cp[i] = new Pair((left + right) / 2 - temp[i-1].x, space);
        //            if (i==0) cp[0] = (left+right)/2 - fov.Rows/2;
        //            else cp[i] = (left + right) / 2 - temp[i-1];
        //        }

        //    } 

        //    return cp;
        //}

        public static void SaveToTextFile(string filePath)
        {
            StreamWriter writer = new StreamWriter(Path.GetFileName(filePath));

            writer.WriteLine(imagePath);
            
            writer.WriteLine(startPos.x.ToString()+"\t"+startPos.y.ToString());
            writer.WriteLine(finishPos.x.ToString() + "\t" + finishPos.y.ToString());

            for (int i = 0; i < fields.GetLength(0); ++i)
            {
                for (int j = 0; j < fields.GetLength(1); ++j)
                {
                    writer.Write(fields[i, j] + "\t");
                }
                writer.WriteLine();
            }

            writer.Close();
        }

        public static void LoadFromTextFile(string filePath)
        {
            //valalmi
            StreamReader reader = new StreamReader(filePath);

            string line = "";
            List<string[]> lineArray = new List<string[]>();

            line = reader.ReadLine().Trim();
            imagePath = line;
            System.Drawing.Bitmap source = new System.Drawing.Bitmap(System.Windows.Forms.Application.StartupPath + "\\Data\\" + imagePath);
            mapHeight = source.Height;
            mapWidth = source.Width;

            line = reader.ReadLine().Trim();
            startPos.x = int.Parse(line.Split('\t')[0]);
            startPos.y = int.Parse(line.Split('\t')[1]);

            line = reader.ReadLine().Trim();
            finishPos.x = int.Parse(line.Split('\t')[0]);
            finishPos.y = int.Parse(line.Split('\t')[1]);

            while (!reader.EndOfStream)
            {
                line = reader.ReadLine().Trim();
                if (line == "")
                    continue;
                lineArray.Add(line.Split('\t'));
            }

            fields = new int[lineArray.Count, lineArray[0].Length];

            for (int i = 0; i < lineArray.Count; ++i)
            {
                for (int j = 0; j < lineArray[0].Length; ++j)
                {
                    fields[i, j] = int.Parse(lineArray[i][j]);
                }
            }

            InitSurfaces();

            reader.Close();
        }
    }    
}
